# pragma once

# include <string.h>
# include <sys/ioctl.h>
# include <fcntl.h>
# include <asm/termbits.h> /* struct termios2 */
# include <signal.h>
# include <chrono>
# include <stdio.h>

#define CANUSB_TTY_BAUD_RATE_DEFAULT 2000000

namespace usb_can_A
{
    typedef enum
    {
        CANUSB_SPEED_1000000 = 0x01,
        CANUSB_SPEED_800000 = 0x02,
        CANUSB_SPEED_500000 = 0x03,
        CANUSB_SPEED_400000 = 0x04,
        CANUSB_SPEED_250000 = 0x05,
        CANUSB_SPEED_200000 = 0x06,
        CANUSB_SPEED_125000 = 0x07,
        CANUSB_SPEED_100000 = 0x08,
        CANUSB_SPEED_50000 = 0x09,
        CANUSB_SPEED_20000 = 0x0a,
        CANUSB_SPEED_10000 = 0x0b,
        CANUSB_SPEED_5000 = 0x0c,
    } SPEED;
    
    enum class MODE
    {
        NORMAL = 0x00,
        LOOPBACK = 0x01,
        SILENT = 0x02,
        LOOPBACK_SILENT = 0x03,
    };
    
    enum class FRAME
    {
        STANDARD = 0x01,
        EXTENDED = 0x02,
    };
    
    SPEED int_to_speed(int speed = -1)
    {
        switch (speed)
        {
        case 1000000:
            return CANUSB_SPEED_1000000;
        case 800000:
            return CANUSB_SPEED_800000;
        case 500000:
            return CANUSB_SPEED_500000;
        case 400000:
            return CANUSB_SPEED_400000;
        case 250000:
            return CANUSB_SPEED_250000;
        case 200000:
            return CANUSB_SPEED_200000;
        case 125000:
            return CANUSB_SPEED_125000;
        case 100000:
            return CANUSB_SPEED_100000;
        case 50000:
            return CANUSB_SPEED_50000;
        case 20000:
            return CANUSB_SPEED_20000;
        case 10000:
            return CANUSB_SPEED_10000;
        case 5000:
            return CANUSB_SPEED_5000;
        default:
            return CANUSB_SPEED_500000;
        }
    }
    
    int generate_checksum(const unsigned char *data, int data_len)
    {
        int i, checksum;
    
        checksum = 0;
        for (i = 0; i < data_len; i++)
        {
            checksum += data[i];
        }
    
        return checksum & 0xff;
    }
    
    int frame_is_complete(const unsigned char *frame, int frame_len)
    {
        if (frame_len > 0)
        {
            if (frame[0] != 0xaa)
            {
                /* Need to sync on 0xaa at start of frames, so just skip. */
                return 1;
            }
        }
    
        if (frame_len < 2)
        {
            return 0;
        }
    
        if (frame[1] == 0x55)
        { /* Command frame... */
            if (frame_len >= 20)
            { /* ...always 20 bytes. */
                return 1;
            }
            else
            {
                return 0;
            }
        }
        else if ((frame[1] >> 4) == 0xc)
        { /* Data frame... */
            if (frame_len >= (frame[1] & 0xf) + 5)
            { /* ...payload and 5 bytes. */
                return 1;
            }
            else
            {
                return 0;
            }
        }
    
        /* Unhandled frame type. */
        return 1;
    }
    
    int frame_send(int tty_fd, const unsigned char *frame, int frame_len, bool print_traffic = false)
    {
        int result, i;
    
        if (print_traffic)
        {
            printf(">>> ");
            for (i = 0; i < frame_len; i++)
            {
                printf("%02x ", frame[i]);
            }
            if (print_traffic > 1)
            {
                printf("    '");
                for (i = 4; i < frame_len - 1; i++)
                {
                    printf("%c", isalnum(frame[i]) ? frame[i] : '.');
                }
                printf("'");
            }
            printf("\n");
        }
    
        result = write(tty_fd, frame, frame_len);
        if (result == -1)
        {
            fprintf(stderr, "write() failed: %s\n", strerror(errno));
            return -1;
        }
    
        return frame_len;
    }
    
    /// @brief recive a frame from can-usb
    /// @return -1 if error, otherwise frame length
    int frame_recv(int tty_fd, unsigned char *frame, int frame_len_max, bool block = false, bool print_traffic = false)
    {
        int result;
        int frame_len;
        int checksum;
        unsigned char byte;
    
        frame_len = 0;
        while (1)
        {
            result = read(tty_fd, &byte, 1);
            if (result <= 0)
            {
                if (!block)
                {
                    return 0;
                }

                if (result == -1)
                {
                    if (errno != EAGAIN && errno != EWOULDBLOCK)
                    {
                        fprintf(stderr, "read() failed: %s\n", strerror(errno));
                        return -1;
                    }
                }
            }
            else if (result > 0)
            {
                if (print_traffic)
                {
                    if (frame_len == 0)
                    {
                        fprintf(stderr, "<<< ");
                    }
                    
                    fprintf(stderr, "%02x ", byte);
                }
    
                if (frame_len == frame_len_max)
                {
                    fprintf(stderr, "frame_recv() failed: Overflow\n");
                    return -1;
                }
    
                frame[frame_len++] = byte;
    
                if (frame_is_complete(frame, frame_len))
                {
                    break;
                }
            }
    
            usleep(10);
        }
    
        if (print_traffic)
            fprintf(stderr, "\n");
    
        /* Compare checksum for command frames only. */
        if ((frame_len == 20) && (frame[0] == 0xaa) && (frame[1] == 0x55))
        {
            checksum = generate_checksum(&frame[2], 17);
            if (checksum != frame[frame_len - 1])
            {
                fprintf(stderr, "frame_recv() failed: Checksum incorrect\n");
                return -1;
            }
        }
    
        return frame_len;
    }

    int hex_value(int c)
    {
        if (c >= 0x30 && c <= 0x39) /* '0' - '9' */
            return c - 0x30;
        else if (c >= 0x41 && c <= 0x46) /* 'A' - 'F' */
            return (c - 0x41) + 10;
        else if (c >= 0x61 && c <= 0x66) /* 'a' - 'f' */
            return (c - 0x61) + 10;
        else
            return -1;
    }
    
    /// @brief send data to can-usb
    /// @return 1 if succeed
    bool send_data_frame(int tty_fd, FRAME frame, char * id, unsigned char * data, int data_length_code, bool print_traffic = false)
    {
        unsigned char id_lsb;
        unsigned char id_msb;

        switch (strlen(id))
        {
        case 1:
            id_lsb = hex_value(id[0]);
            break;

        case 2:
            id_lsb = (hex_value(id[0]) * 16) + hex_value(id[1]);
            break;

        case 3:
            id_msb = hex_value(id[0]);
            id_lsb = (hex_value(id[1]) * 16) + hex_value(id[2]);
            break;

        default:
            fprintf(stderr, "Unable to convert ID from hex to binary!\n");
            return 0;
        }

        const int max_frame_size = 13;
        int data_frame_len = 0;
        unsigned char data_frame[max_frame_size] = {0x00};
    
        if (data_length_code < 0 || data_length_code > 8)
        {
            fprintf(stderr, "Data length code (DLC) must be between 0 and 8!\n");
            return 0;
        }
    
        /* Byte 0: Packet Start */
        data_frame[data_frame_len++] = 0xaa;
    
        /* Byte 1: CAN Bus Data Frame Information */
        data_frame[data_frame_len] = 0x00;
        data_frame[data_frame_len] |= 0xC0; /* Bit 7 Always 1, Bit 6 Always 1 */
        if (frame == FRAME::STANDARD)
        {
            data_frame[data_frame_len] &= 0xDF;         /* STD frame */
        }
        else                                            /* CANUSB_FRAME_EXTENDED */
        {
            data_frame[data_frame_len] |= 0x20;         /* EXT frame */
        }
        data_frame[data_frame_len] &= 0xEF;             /* 0=Data */
        data_frame[data_frame_len] |= data_length_code; /* DLC=data_len */
        data_frame_len++;
    
        /* Byte 2 to 3: ID */
        data_frame[data_frame_len++] = id_lsb; /* lsb */
        data_frame[data_frame_len++] = id_msb; /* msb */
    
        /* Byte 4 to (4+data_len): Data */
        for (int i = 0; i < data_length_code; i++)
        {
            data_frame[data_frame_len++] = data[i];
        }
    
        /* Last byte: End of frame */
        data_frame[data_frame_len++] = 0x55;
    
        if (frame_send(tty_fd, data_frame, data_frame_len, print_traffic) < 0)
        {
            fprintf(stderr, "Unable to send frame!\n");
            return 0;
        }
    
        return 1;
    }

    /// @brief recive data from can-usb, only for standard frame
    /// @return 1 if recived
    bool recv_data_stdframe(int tty_fd, uint32_t & recv_id, uint8_t * recv_data, uint8_t & recv_len, bool block = false, bool print_traffic = false)
    {
        unsigned char frame[32];

        int frame_len = frame_recv(tty_fd, frame, sizeof(frame), block, print_traffic);
        recv_len = 0;

        if (frame_len == -1)
        {
            printf("Frame recieve error!\n");
        }
        else if (frame_len > 0)
        {

            if ((frame_len >= 6) &&
                (frame[0] == 0xaa) &&
                ((frame[1] >> 4) == 0xc))
            {
                recv_id = ((uint32_t)(uint8_t)frame[3] << 8) | (uint32_t)(uint8_t)frame[2];

                recv_len = frame_len - 5;

                memcpy(recv_data, frame + 4, recv_len);

                return 1;
            }
            else
            {
                printf("Unknown: ");
                for (size_t i = 0; i <= frame_len; i++)
                {
                    printf("%02x ", frame[i]);
                }
                printf("\n");
            }
        }

        return 0;
    }
    
    int command_settings(int tty_fd, SPEED speed = SPEED::CANUSB_SPEED_500000, MODE mode = MODE::NORMAL, FRAME frame = FRAME::STANDARD)
    {
        int cmd_frame_len;
        unsigned char cmd_frame[20];
    
        cmd_frame_len = 0;
        cmd_frame[cmd_frame_len++] = 0xaa;
        cmd_frame[cmd_frame_len++] = 0x55;
        cmd_frame[cmd_frame_len++] = 0x12;
        cmd_frame[cmd_frame_len++] = speed;
        cmd_frame[cmd_frame_len++] = static_cast<char>(frame);
        cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
        cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
        cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
        cmd_frame[cmd_frame_len++] = 0; /* Filter ID not handled. */
        cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
        cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
        cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
        cmd_frame[cmd_frame_len++] = 0; /* Mask ID not handled. */
        cmd_frame[cmd_frame_len++] = static_cast<char>(mode);
        cmd_frame[cmd_frame_len++] = 0x01;
        cmd_frame[cmd_frame_len++] = 0;
        cmd_frame[cmd_frame_len++] = 0;
        cmd_frame[cmd_frame_len++] = 0;
        cmd_frame[cmd_frame_len++] = 0;
        cmd_frame[cmd_frame_len++] = generate_checksum(&cmd_frame[2], 17);
    
        if (frame_send(tty_fd, cmd_frame, cmd_frame_len) < 0)
        {
            return -1;
        }
    
        return 0;
    }
    
    int adapter_init(const char *tty_device, int baudrate)
    {
        int tty_fd, result;
        struct termios2 tio;
    
        tty_fd = open(tty_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
        if (tty_fd == -1)
        {
            fprintf(stderr, "open(%s) failed: %s\n", tty_device, strerror(errno));
            return -1;
        }
    
        result = ioctl(tty_fd, TCGETS2, &tio);
        if (result == -1)
        {
            fprintf(stderr, "ioctl() failed: %s\n", strerror(errno));
            close(tty_fd);
            return -1;
        }
    
        tio.c_cflag &= ~CBAUD;
        tio.c_cflag = BOTHER | CS8 | CSTOPB;
        tio.c_iflag = IGNPAR;
        tio.c_oflag = 0;
        tio.c_lflag = 0;
        tio.c_ispeed = baudrate;
        tio.c_ospeed = baudrate;
    
        result = ioctl(tty_fd, TCSETS2, &tio);
        if (result == -1)
        {
            fprintf(stderr, "ioctl() failed: %s\n", strerror(errno));
            close(tty_fd);
            return -1;
        }
    
        return tty_fd;
    }
} // namespace usb_canA
